
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       sim_ship.c
  * @author     baiyang
  * @date       2022-11-30
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "sim_ship.h"

#if AP_SIM_SHIP_ENABLED
#include "sitl.h"
#include "sim_aircraft.h"

#include <stdio.h>
#include <string.h>

#include <parameter/param.h>
#include <gcs_mavlink/gcs.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static void send_report(sim_ship_t simship);
/*----------------------------------variable----------------------------------*/
// SITL Ship parameters
static param_t var_info[] = {
    PARAM_DEFINE_INT8(SIM_SHIP_ENABLE, 0),
    PARAM_DEFINE_FLOAT(SIM_SHIP_SPEED, 3),
    PARAM_DEFINE_FLOAT(SIM_SHIP_PSIZE, 1000),
    PARAM_DEFINE_INT8(SIM_SHIP_SYSID,  17),
    PARAM_DEFINE_FLOAT(SIM_SHIP_DSIZE, 10),
    PARAM_DEFINE_FLOAT(SIM_SHIP_OFS_X, 0),
    PARAM_DEFINE_FLOAT(SIM_SHIP_OFS_Y, 0),
    PARAM_DEFINE_FLOAT(SIM_SHIP_OFS_Z, 0),
};
PARAM_GROUP_DEFINE(SIM_SHIP, var_info);


// use a spare channel for send. This is static to avoid mavlink
// header import in SIM_Ship.h
static const mavlink_channel_t mavlink_ch = (mavlink_channel_t)(MAVLINK_COMM_0+6);
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void sim_ship_ctor(sim_ship_t simship)
{
    memset(simship, 0, sizeof(struct sim_ship));

    param_link_variable(PARAM_ID(SIM_SHIP, SIM_SHIP_ENABLE), &simship->enable);
    param_link_variable(PARAM_ID(SIM_SHIP, SIM_SHIP_SPEED),  &simship->speed);
    param_link_variable(PARAM_ID(SIM_SHIP, SIM_SHIP_PSIZE),  &simship->path_size);
    param_link_variable(PARAM_ID(SIM_SHIP, SIM_SHIP_SYSID),  &simship->sys_id);
    param_link_variable(PARAM_ID(SIM_SHIP, SIM_SHIP_DSIZE),  &simship->deck_size);
    param_link_variable(PARAM_ID(SIM_SHIP, SIM_SHIP_OFS_X),  &simship->offset.x);
    param_link_variable(PARAM_ID(SIM_SHIP, SIM_SHIP_OFS_Y),  &simship->offset.y);
    param_link_variable(PARAM_ID(SIM_SHIP, SIM_SHIP_OFS_Z),  &simship->offset.z);

    simship->target_address = "127.0.0.1";
    simship->target_port    = 5762;

    simship->reporting_period_ms = 200;

    simship->ship.sim = simship;

    mb_socket_ctor(&simship->mav_socket, false);
}

/*
  update a simulated vehicle
 */
void ship_update(ship_t ship, float delta_t)
{
    if (!ship->sim) {
        return;
    }

    // acceletate over time to keep EKF happy
    const float max_accel = 3.0;
    const float dspeed_max = max_accel * delta_t;
    ship->speed = math_constrain_float(ship->sim->speed, ship->speed-dspeed_max, ship->speed+dspeed_max);

    // calculate how far around the circle we go
    float circumference = M_PI * ship->sim->path_size;
    float dist = delta_t * ship->speed;
    float dangle = (dist / circumference) * 360.0f;

    if (delta_t > 0) {
        ship->yaw_rate = radians(dangle) / delta_t;
    }
    ship->heading_deg += dangle;
    ship->heading_deg = math_wrap_360(ship->heading_deg);

    Vector2f_t dpos = {dist, 0};
    vec2_rotate(&dpos, radians(ship->heading_deg));

    ship->position.x += dpos.x;
    ship->position.y += dpos.y;
}

/*
  get ground speed adjustment if we are landed on the ship
 */
Vector2f_t sim_ship_get_ground_speed_adjustment(sim_ship_t simship, const Location *loc, float *yaw_rate)
{
    Vector2f_t vel = {0,0};

    if (!simship->enable) {
        *yaw_rate = 0;
        return vel;
    }
    Location shiploc = simship->home;
    location_offset(&shiploc, simship->ship.position.x, simship->ship.position.y);
    if (location_get_distance(loc, &shiploc) > simship->deck_size) {
        *yaw_rate = 0;
        return vel;
    }

    // find center of the circle that the ship is on
    Location center = shiploc;
    const float path_radius = simship->path_size*0.5f;
    location_offset_bearing(&center, simship->ship.heading_deg+(simship->ship.yaw_rate>0?90:-90), path_radius);

    // scale speed for ratio of distances
    const float p = location_get_distance(&center, loc) / path_radius;
    const float scaled_speed = simship->ship.speed * p;

    // work out how far around the circle ahead or behind we are for
    // rotating velocity
    const float bearing1 = location_get_bearing(&center, loc);
    const float bearing2 = location_get_bearing(&center, &shiploc);
    const float heading = simship->ship.heading_deg + degrees(bearing1-bearing2);

    vel.x = scaled_speed;
    vel.y = 0.0f;

    vec2_rotate(&vel, radians(heading));
    *yaw_rate = simship->ship.yaw_rate;
    return vel;
}

/*
  update the ShipSim peripheral state
*/
void sim_ship_update(sim_ship_t simship)
{
    if (!simship->enable) {
        return;
    }

    sim_t sitl = sim_get_singleton();

    uint32_t now_us = time_micros();

    if (!simship->initialised) {
        simship->home = sitl->state.home;
        if (simship->home.lat == 0 && simship->home.lng == 0) {
            return;
        }
        const Vector3f_t *ofs = &simship->offset;
        location_offset(&simship->home, ofs->x, ofs->y);
        simship->home.alt -= ofs->z*100;

        simship->initialised = true;
        printf("ShipSim home %f %f\n", simship->home.lat*1.0e-7, simship->home.lng*1.0e-7);
        simship->ship.sim = simship;
        simship->last_update_us = now_us;
        simship->last_report_ms = time_millis();
    }

    float dt = (now_us - simship->last_update_us)*1.0e-6;
    simship->last_update_us = now_us;

    ship_update(&simship->ship, dt);

    uint32_t now_ms = time_millis();
    if (now_ms - simship->last_report_ms >= simship->reporting_period_ms) {
        simship->last_report_ms = now_ms;
        send_report(simship);
    }
}

/*
  send a report to the vehicle control code over MAVLink
*/
static void send_report(sim_ship_t simship)
{
    if (!simship->mavlink_connected && mb_socket_connect(&simship->mav_socket, simship->target_address, simship->target_port)) {
        printf("ShipSim connected to %s:%u\n", simship->target_address, (unsigned)simship->target_port);
        simship->mavlink_connected = true;
    }
    if (!simship->mavlink_connected) {
        return;
    }

    uint32_t now = time_millis();
    mavlink_message_t msg;
    uint16_t len = 0;
    uint8_t buf[300] = {0};

    const uint8_t component_id = MAV_COMP_ID_USER10;

    if (now - simship->last_heartbeat_ms >= 1000) {
        simship->last_heartbeat_ms = now;
        mavlink_msg_heartbeat_pack_chan(simship->sys_id,
                                        component_id,
                                        mavlink_ch,
                                        &msg,
                                        MAV_TYPE_SURFACE_BOAT,
                                        MAV_AUTOPILOT_INVALID,
                                        0,
                                        0,
                                        0);
        len = mavlink_msg_to_send_buffer(buf, &msg);
        mb_socket_send(&simship->mav_socket, buf, len);
    }


    /*
      send a GLOBAL_POSITION_INT messages
     */
    Location loc = simship->home;
    location_offset(&loc, simship->ship.position.x, simship->ship.position.y);

    int32_t alt_mm = simship->home.alt * 10;  // assume home altitude

#if AP_TERRAIN_AVAILABLE
    auto terrain = AP::terrain();
    float height;
    if (terrain != nullptr && terrain->enabled() && terrain->height_amsl(loc, height, false)) {
        alt_mm = height * 1000;
    }
#endif

    Vector2f_t vel = {simship->ship.speed, 0};
    vec2_rotate(&vel, radians(simship->ship.heading_deg));

    mavlink_msg_global_position_int_pack_chan(simship->sys_id,
                                              component_id,
                                              mavlink_ch,
                                              &msg,
                                              now,
                                              loc.lat,
                                              loc.lng,
                                              alt_mm,
                                              0,
                                              vel.x*100,
                                              vel.y*100,
                                              0,
                                              simship->ship.heading_deg*100);
    len = mavlink_msg_to_send_buffer(buf, &msg);
    if (len > 0) {
        mb_socket_send(&simship->mav_socket, buf, len);
    }

    // also set ATTITUDE so MissionPlanner can display ship orientation
    mavlink_msg_attitude_pack_chan(simship->sys_id,
                                   component_id,
                                   mavlink_ch,
                                   &msg,
                                   now,
                                   0, 0, radians(simship->ship.heading_deg),
                                   0, 0, simship->ship.yaw_rate);
    len = mavlink_msg_to_send_buffer(buf, &msg);
    if (len > 0) {
        mb_socket_send(&simship->mav_socket, buf, len);
    }
}

/*------------------------------------test------------------------------------*/
#endif

